#include "arm.h"

Arm::Arm(QObject *parent) :
    Object3D(parent)
{
    calcQA.setScalar(sqrt(0.5));
    calcQA.setX(-sqrt(0.5));
    calcQA.setY(0);
    calcQA.setZ(0);
    calcQB.setScalar(sqrt(0.5));
    calcQB.setX(0);
    calcQB.setY(0);
    calcQB.setZ(sqrt(0.5));
    calcQ = calcQA*calcQB;

    calcQ1.setScalar(sqrt(0.5));
    calcQ1.setX(0);
    calcQ1.setY(sqrt(0.5));
    calcQ1.setZ(0);

    calcQ2.setScalar(sqrt(0.5));
    calcQ2.setX(sqrt(0.5));
    calcQ2.setY(0);
    calcQ2.setZ(0);

    body = new Mesh3D(this);
    body->loadModel(":/model3");
    body->setCenter(155,0,0);
    body->setOrient(-20,0,0);

    joint1 = new Mesh3D(this);
    joint1->loadModel(":/model2");
    joint1->setCenter(0,0,0);
    joint1->setOrient(-20,0,0);

    shoulder = new Mesh3D(joint1);
    shoulder->loadModel(":/model1");
    shoulder->setCenter(-110.8,0,0);
    shoulder->setOrient(90,20,0);
    shoulder->setColor(QColor());

    joint2 = new Mesh3D(shoulder);
    joint2->loadModel(":/model2");
//    joint2->setOrient(0,0,0);
    joint2->setOrient(180,90,0);
    joint2->setPosition(-171,0,0);
//    joint2->setCenter(-171,0,0);
//    joint2->setOrient(180,180,45);
    joint2->showAxes(true);
//    joint2->setVisible(false);


    forearm = new Mesh3D(joint2);
    forearm->loadModel(":/model4");
    forearm->setCenter(-110,0,0);
//    forearm->setOrient(0,0,0);
//    forearm->setVisible(false);
}

Arm::~Arm()
{
}

void Arm::setJoint1Orient(const QMatrix4x4 &mat)
{
    float m[16];
    for(int i=0;i<16;i++)
        m[i] = mat.data()[i];

    joint1->setTransform(m);
}

void Arm::setJoint1Orient(const QQuaternion &q)
{
     joint1->setRotation(q);
}

void Arm::setShoulderOrient(const QMatrix4x4 &mat)
{
    float m[16];
    for(int i=0;i<16;i++)
        m[i] = mat.data()[i];
    shoulder->setTransform(m);
}

void Arm::setShoulderOrient(const QQuaternion &q)
{
    QQuaternion cQ1,cQ2, cQ, newQ;

    cQ.setScalar(sqrt(0.5));
    cQ.setX(-sqrt(0.5));
    cQ.setY(0);
    cQ.setZ(0);
    newQ = cQ*q*(cQ.conjugate()/cQ.length());


    cQ1.setScalar(sqrt(0.5));
    cQ1.setX(0);
    cQ1.setY(sqrt(0.5));
    cQ1.setZ(0);

    cQ2.setScalar(sqrt(0.5));
    cQ2.setX(sqrt(0.5));
    cQ2.setY(0);
    cQ2.setZ(0);

    shoulder->setRotation(cQ1*cQ2*newQ);
}

void Arm::setCallibrate(int &val)
{
//    forearm->setCenter(-110+val,0,0);
}

void Arm::setJoint2Orient(const QMatrix4x4 &mat)
{
    float m[16];
    for(int i=0;i<16;i++)
        m[i] = mat.data()[i];

    joint2->setTransform(m);
    joint2->setPosition(-171,0,0);
}

void Arm::setJoint2Orient(const QQuaternion &q)
{
    joint2->setRotation(q);
}

void Arm::setForearmOrient(const QMatrix4x4 &mat)
{
    float m[16];
    for(int i=0;i<16;i++)
        m[i] = mat.data()[i];
    forearm->setTransform(m);
}

void Arm::setForearmOrient(const QQuaternion &q)
{
    forearm->setRotation(q);
}

void Arm::setOrient(const QQuaternion &q1, const QQuaternion &q2)
{
    QQuaternion cQ1,cQ2, cQA, cQB, cQ, newQ;

    cQA.setScalar(sqrt(0.5));
    cQA.setX(-sqrt(0.5));
    cQA.setY(0);
    cQA.setZ(0);
    cQB.setScalar(sqrt(0.5));
    cQB.setX(0);
    cQB.setY(0);
    cQB.setZ(sqrt(0.5));
    cQ = cQA*cQB;
    newQ = cQ*q1*(cQ.conjugate()/cQ.length());

    cQ1.setScalar(sqrt(0.5));
    cQ1.setX(0);
    cQ1.setY(sqrt(0.5));
    cQ1.setZ(0);

    cQ2.setScalar(sqrt(0.5));
    cQ2.setX(sqrt(0.5));
    cQ2.setY(0);
    cQ2.setZ(0);

    shoulder->setRotation(cQ1*cQ2*newQ);
    forearm->setRotation(cQ2*q2);
}

void Arm::setOrient(const QVector<float> &vector)
{
    joint1->setOrient(-20, 90-vector.at(1), 0);
    shoulder->setOrient(90, -vector.at(0)+20, 0);
    joint2->setOrient(180, 90+vector.at(3), 0);
    forearm->setOrient(90, -vector.at(2), 0);
}

void Arm::setOrient(int ind, QQuaternion quat)
{
    if(ind==0)
    {
        QQuaternion newQ;
        newQ = calcQ*quat*(calcQ.conjugate()/calcQ.length());
        shoulder->setRotation(calcQ1*calcQ2*newQ);
    }
    else if(ind==1)
        forearm->setRotation(calcQ2*quat);
}

void Arm::setOrient(int ind, float ang1, float ang2)
{
    if(ind==0)
    {
        joint1->setOrient(-20, 90-ang2, 0);
        shoulder->setOrient(90, -ang1+20, 0);
    }
    else if(ind==1)
    {
        joint2->setOrient(180, 90+ang2, 0);
        forearm->setOrient(90, -ang1, 0);
    }
}

void Arm::setRotByButton()
{
    joint2->rotate(30,0,1,0);
}
